
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_rangefinder.c
  * @author     baiyang
  * @date       2023-2-9
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_rangefinder.h"
#include "sensor_rf_benewake_tfmini.h"
#include "sensor_rf_benewake_tf02.h"
#include "sensor_rf_benewake_tf03.h"

#include <common/console/console.h>
#include <internal_error/internal_error.h>
#include <common/microbee.h>
#include <logger/blog.h>
#include <logger/log_structure.h>
/*-----------------------------------macro------------------------------------*/
// @Param: TYPE
// @DisplayName: Rangefinder type
// @Description: Type of connected rangefinder
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,100:SITL
// @User: Standard

// @Param: PIN
// @DisplayName: Rangefinder pin
// @Description: Analog or PWM input pin that rangefinder is connected to. Airspeed ports can be used for Analog input, AUXOUT can be used for PWM input. When using analog pin 103, the maximum value of the input in 3.3V. For PWM input, the pin must be configured as a digital GPIO, see the Wiki's "GPIOs" section for details.
// @Values: -1:Not Used,11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6,103:Pixhawk SBUS
// @User: Standard

// @Param: SCALING
// @DisplayName: Rangefinder scaling
// @Description: Scaling factor between rangefinder reading and distance. For the linear and inverted functions this is in meters per volt. For the hyperbolic function the units are meterVolts. For Maxbotix serial sonar this is unit conversion to meters.
// @Units: m/V
// @Increment: 0.001
// @User: Standard

// @Param: OFFSET
// @DisplayName: rangefinder offset
// @Description: Offset in volts for zero distance for analog rangefinders. Offset added to distance in centimeters for PWM lidars
// @Units: V
// @Increment: 0.001
// @User: Standard

// @Param: FUNCTION
// @DisplayName: Rangefinder function
// @Description: Control over what function is used to calculate distance. For a linear function, the distance is (voltage-offset)*scaling. For a inverted function the distance is (offset-voltage)*scaling. For a hyperbolic function the distance is scaling/(voltage-offset). The functions return the distance in meters.
// @Values: 0:Linear,1:Inverted,2:Hyperbolic
// @User: Standard

// @Param: MIN_CM
// @DisplayName: Rangefinder minimum distance
// @Description: Minimum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Standard

// @Param: MAX_CM
// @DisplayName: Rangefinder maximum distance
// @Description: Maximum distance in centimeters that rangefinder can reliably read
// @Units: cm
// @Increment: 1
// @User: Standard

// @Param: STOP_PIN
// @DisplayName: Rangefinder stop pin
// @Description: Digital pin that enables/disables rangefinder measurement for the pwm rangefinder. A value of -1 means no pin. If this is set, then the pin is set to 1 to enable the rangefinder and set to 0 to disable it. This is used to enable powersaving when out of range. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot.
// @Values: -1:Not Used,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6,111:PX4 FMU Relay1,112:PX4 FMU Relay2,113:PX4IO Relay1,114:PX4IO Relay2,115:PX4IO ACC1,116:PX4IO ACC2
// @User: Standard

// 9 was SETTLE

// @Param: RMETRIC
// @DisplayName: Ratiometric
// @Description: This parameter sets whether an analog rangefinder is ratiometric. Most analog rangefinders are ratiometric, meaning that their output voltage is influenced by the supply voltage. Some analog rangefinders (such as the SF/02) have their own internal voltage regulators so they are not ratiometric.
// @Values: 0:No,1:Yes
// @User: Standard

// @Param: PWRRNG
// @DisplayName: Powersave range
// @Description: This parameter sets the estimated terrain distance in meters above which the sensor will be put into a power saving mode (if available). A value of zero means power saving is not enabled
// @Units: m
// @Range: 0 32767
// @User: Standard

// @Param: GNDCLEAR
// @DisplayName: Distance (in cm) from the range finder to the ground
// @Description: This parameter sets the expected range measurement(in cm) that the range finder should return when the vehicle is on the ground.
// @Units: cm
// @Range: 5 127
// @Increment: 1
// @User: Standard

// @Param: ADDR
// @DisplayName: Bus address of sensor
// @Description: This sets the bus address of the sensor, where applicable. Used for the I2C and DroneCAN sensors to allow for multiple sensors on different addresses.
// @Range: 0 127
// @Increment: 1
// @User: Standard

// @Param: POS_X
// @DisplayName:  X position offset
// @Description: X position of the rangefinder in body frame. Positive X is forward of the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced

// @Param: POS_Y
// @DisplayName: Y position offset
// @Description: Y position of the rangefinder in body frame. Positive Y is to the right of the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced

// @Param: POS_Z
// @DisplayName: Z position offset
// @Description: Z position of the rangefinder in body frame. Positive Z is down from the origin. Use the zero range datum point if supplied.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced

// @Param: ORIENT
// @DisplayName: Rangefinder orientation
// @Description: Orientation of rangefinder
// @Values: 0:Forward, 1:Forward-Right, 2:Right, 3:Back-Right, 4:Back, 5:Back-Left, 6:Left, 7:Forward-Left, 24:Up, 25:Down
// @User: Advanced

#define RF_PARAM_VAL_INFO(x) \
    PARAM_DEFINE_INT8(RNGFND##x##_TYPE, 0), \
    PARAM_DEFINE_INT8(RNGFND##x##_PIN, -1), \
    PARAM_DEFINE_FLOAT(RNGFND##x##_SCALING, 3.0f), \
    PARAM_DEFINE_FLOAT(RNGFND##x##_OFFSET, 0), \
    PARAM_DEFINE_INT8(RNGFND##x##_FUNCTION, 0), \
    PARAM_DEFINE_INT16(RNGFND##x##_MIN_CM, 20), \
    PARAM_DEFINE_INT16(RNGFND##x##_MAX_CM, 700), \
    PARAM_DEFINE_INT8(RNGFND##x##_STOP_PIN, -1), \
    PARAM_DEFINE_INT8(RNGFND##x##_RMETRIC, 1), \
    PARAM_DEFINE_INT16(RNGFND##x##_PWRRNG, 0), \
    PARAM_DEFINE_INT8(RNGFND##x##_GNDCLEAR, RANGEFINDER_GROUND_CLEARANCE_CM_DEFAULT), \
    PARAM_DEFINE_INT8(RNGFND##x##_ADDR, 0), \
    PARAM_DEFINE_FLOAT(RNGFND##x##_POS_X, 0), \
    PARAM_DEFINE_FLOAT(RNGFND##x##_POS_Y, 0), \
    PARAM_DEFINE_FLOAT(RNGFND##x##_POS_Z, 0), \
    PARAM_DEFINE_INT8(RNGFND##x##_ORIENT, 0)

#define RF_PARAM_LINK_VARIABLE(rf, n) \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_TYPE), &rf.params[n-1].type); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_PIN), &rf.params[n-1].pin); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_SCALING), &rf.params[n-1].scaling); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_OFFSET), &rf.params[n-1].offset); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_FUNCTION), &rf.params[n-1].function); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_MIN_CM), &rf.params[n-1].min_distance_cm); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_MAX_CM), &rf.params[n-1].max_distance_cm); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_STOP_PIN), &rf.params[n-1].stop_pin); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_RMETRIC), &rf.params[n-1].ratiometric); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_PWRRNG), &rf.params[n-1].powersave_range); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_GNDCLEAR), &rf.params[n-1].ground_clearance_cm); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_ADDR), &rf.params[n-1].address); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_POS_X),&rf.params[n-1].pos_offset.x); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_POS_Y), &rf.params[n-1].pos_offset.y); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_POS_Z), &rf.params[n-1].pos_offset.z); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_ORIENT), &rf.params[n-1].orientation)

#define RF_PARAM_LINK_VARIABLE2(rf, n, idx) \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_TYPE), &rf.params[idx-1].type); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_PIN), &rf.params[idx-1].pin); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_SCALING), &rf.params[idx-1].scaling); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_OFFSET), &rf.params[idx-1].offset); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_FUNCTION), &rf.params[idx-1].function); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_MIN_CM), &rf.params[idx-1].min_distance_cm); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_MAX_CM), &rf.params[idx-1].max_distance_cm); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_STOP_PIN), &rf.params[idx-1].stop_pin); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_RMETRIC), &rf.params[idx-1].ratiometric); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_PWRRNG), &rf.params[idx-1].powersave_range); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_GNDCLEAR), &rf.params[idx-1].ground_clearance_cm); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_ADDR), &rf.params[idx-1].address); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_POS_X),&rf.params[idx-1].pos_offset.x); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_POS_Y), &rf.params[idx-1].pos_offset.y); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_POS_Z), &rf.params[idx-1].pos_offset.z); \
    param_link_variable(PARAM_ID(RNGFND, RNGFND##n##_ORIENT), &rf.params[idx-1].orientation)

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool _add_backend(sensor_rf_backend_t backend, uint8_t instance, uint8_t serial_instance);
static void detect_instance(uint8_t instance, uint8_t* serial_instance);
static void sensor_rngfnd_assign_param();
/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    RF_PARAM_VAL_INFO(1),
#if RANGEFINDER_MAX_INSTANCES > 1
    RF_PARAM_VAL_INFO(2),
#endif

#if RANGEFINDER_MAX_INSTANCES > 2
    RF_PARAM_VAL_INFO(3),
#endif

#if RANGEFINDER_MAX_INSTANCES > 3
    RF_PARAM_VAL_INFO(4),
#endif

#if RANGEFINDER_MAX_INSTANCES > 4
    RF_PARAM_VAL_INFO(5),
#endif

#if RANGEFINDER_MAX_INSTANCES > 5
    RF_PARAM_VAL_INFO(6),
#endif

#if RANGEFINDER_MAX_INSTANCES > 6
    RF_PARAM_VAL_INFO(7),
#endif

#if RANGEFINDER_MAX_INSTANCES > 7
    RF_PARAM_VAL_INFO(8),
#endif

#if RANGEFINDER_MAX_INSTANCES > 8
    RF_PARAM_VAL_INFO(9),
#endif

#if RANGEFINDER_MAX_INSTANCES > 9
    RF_PARAM_VAL_INFO(A),
#endif
};
PARAM_GROUP_DEFINE(RNGFND, var_info);

static struct sensor_rangefinder rangefinder;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
int sensor_rngfnd_ctor()
{
    sensor_rngfnd_assign_param();

    rangefinder.detect_sem = rt_mutex_create("rf", RT_IPC_FLAG_FIFO);

    if (!rangefinder.detect_sem) {
        console_panic("rangefinder: create rangefinder.detect_sem fail\n");
    }

    rangefinder._log_rfnd_bit = -1;

    return 0;
}
#if (MB_RANGEFINDER_ENABLED == ENABLED)
MB_INIT_COMPONENT_EXPORT(sensor_rngfnd_ctor);
#endif

/*
  initialise the RangeFinder class. We do detection of attached range
  finders here. For now we won't allow for hot-plugging of
  rangefinders.
 */
void sensor_rngfnd_init()
{
    if (rangefinder.init_done) {
        // init called a 2nd time?
        return;
    }
    rangefinder.init_done = true;

    for (uint8_t i=0, serial_instance = 0; i<RANGEFINDER_MAX_INSTANCES; i++) {
        // serial_instance will be increased inside detect_instance
        // if a serial driver is loaded for this instance
        rt_mutex_take(rangefinder.detect_sem, RT_WAITING_FOREVER);

        detect_instance(i, &serial_instance);
        if (rangefinder.drivers[i] != NULL) {
            // we loaded a driver for this instance, so it must be
            // present (although it may not be healthy). We use MAX()
            // here as a UAVCAN rangefinder may already have been
            // found
            rangefinder.num_instances = MAX(rangefinder.num_instances, i+1);
        }

        // initialise status
        rangefinder.state[i].status = RangeFinderNotConnected;
        rangefinder.state[i].range_valid_count = 0;

        rt_mutex_release(rangefinder.detect_sem);
    }
}

/*
  update RangeFinder state for all instances. This should be called at
  around 10Hz by main loop
 */
void sensor_rngfnd_update(void)
{
    for (uint8_t i=0; i<rangefinder.num_instances; i++) {
        if (rangefinder.drivers[i] != NULL) {
            if ((enum RangeFinderType)rangefinder.params[i].type == RANGEFINDER_NONE) {
                // allow user to disable a rangefinder at runtime
                rangefinder.state[i].status = RangeFinderNotConnected;
                rangefinder.state[i].range_valid_count = 0;
                continue;
            }
            sensor_rf_update(rangefinder.drivers[i]);
        }
    }

    sensor_rngfnd_log_rfnd();
}

void sensor_rngfnd_set_log_rfnd_bit(uint32_t log_rfnd_bit)
{
    rangefinder._log_rfnd_bit = log_rfnd_bit;
}

/*
  Return the number of range finder instances. Note that if users
  sets up rangefinders with a gap in the types then this is the
  index of the maximum sensor ID plus one, so this gives the value
  that should be used when iterating over all sensors
*/
uint8_t sensor_rngfnd_num_sensors(void)
{
    return rangefinder.num_instances;
}

// get rangefinder type for an ID
enum RangeFinderType sensor_rngfnd_get_type(uint8_t id)
{
    return id >= RANGEFINDER_MAX_INSTANCES? RANGEFINDER_NONE : (enum RangeFinderType)(rangefinder.params[id].type);
}

// get rangefinder address (for AP_Periph CAN)
uint8_t sensor_rngfnd_get_address(uint8_t id)
{
    return id >= RANGEFINDER_MAX_INSTANCES? 0 : (uint8_t)(rangefinder.params[id].address);
}

/*
  set an externally estimated terrain height. Used to enable power
  saving (where available) at high altitudes.
 */
void sensor_rngfnd_set_estimated_terrain_height(float height)
{
    rangefinder.estimated_terrain_height = height;
}

sensor_rf_backend_t sensor_rngfnd_get_backend(uint8_t id)
{
    if (id >= rangefinder.num_instances) {
        return NULL;
    }
    if (rangefinder.drivers[id] != NULL) {
        if (sensor_rf_type(rangefinder.drivers[id]) == RANGEFINDER_NONE) {
            // pretend it isn't here; disabled at runtime?
            return NULL;
        }
    }
    return rangefinder.drivers[id];
};

enum RangeFinderStatus sensor_rngfnd_status_orient(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return RangeFinderNotConnected;
    }
    return sensor_rf_status(backend);
}

// find first range finder instance with the specified orientation
sensor_rf_backend_t sensor_rngfnd_find_instance(enum RotationEnum orientation)
{
    // first try for a rangefinder that is in range
    for (uint8_t i=0; i<rangefinder.num_instances; i++) {
        sensor_rf_backend_t backend = sensor_rngfnd_get_backend(i);
        if (backend != NULL &&
            sensor_rf_orientation(backend) == orientation &&
            sensor_rf_status(backend ) == RangeFinderGood) {
            return backend;
        }
    }
    // if none in range then return first with correct orientation
    for (uint8_t i=0; i<rangefinder.num_instances; i++) {
        sensor_rf_backend_t backend = sensor_rngfnd_get_backend(i);
        if (backend != NULL &&
            sensor_rf_orientation(backend) == orientation) {
            return backend;
        }
    }
    return NULL;
}

void sensor_rngfnd_handle_msg(const mavlink_message_t *msg)
{
    uint8_t i;
    for (i=0; i<rangefinder.num_instances; i++) {
        if ((rangefinder.drivers[i] != NULL) && ((enum RangeFinderType)rangefinder.params[i].type != RANGEFINDER_NONE)) {
          sensor_rf_handle_msg(rangefinder.drivers[i], msg);
        }
    }
}

// return true if we have a range finder with the specified orientation
bool sensor_rngfnd_has_orientation(enum RotationEnum orientation)
{
    return (sensor_rngfnd_find_instance(orientation) != NULL);
}

float sensor_rngfnd_distance_orient(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return 0;
    }
    return sensor_rf_distance(backend);
}

uint16_t sensor_rngfnd_distance_cm_orient(enum RotationEnum orientation)
{
    return sensor_rngfnd_distance_orient(orientation) * 100;
}

int16_t sensor_rngfnd_max_distance_cm_orient(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return 0;
    }
    return sensor_rf_max_distance_cm(backend);
}

int16_t sensor_rngfnd_min_distance_cm_orient(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return 0;
    }
    return sensor_rf_min_distance_cm(backend);
}

int16_t sensor_rngfnd_ground_clearance_cm_orient(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return 0;
    }
    return sensor_rf_ground_clearance_cm(backend);
}

bool sensor_rngfnd_has_data_orient(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return false;
    }
    return sensor_rf_has_data(backend);
}

uint8_t sensor_rngfnd_range_valid_count_orient(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return 0;
    }
    return sensor_rf_range_valid_count(backend);
}

const Vector3f_t* sensor_rngfnd_get_pos_offset_orient(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return &rangefinder.pos_offset_zero;
    }
    return sensor_rf_get_pos_offset(backend);
}

uint32_t sensor_rngfnd_last_reading_ms(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return 0;
    }
    return sensor_rf_last_reading_ms(backend);
}

MAV_DISTANCE_SENSOR sensor_rngfnd_get_mav_distance_sensor_type_orient(enum RotationEnum orientation)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return MAV_DISTANCE_SENSOR_UNKNOWN;
    }
    return sensor_rf_get_mav_distance_sensor_type(backend);
}

// get temperature reading in C.  returns true on success and populates temp argument
bool sensor_rngfnd_get_temp(enum RotationEnum orientation, float *temp)
{
    sensor_rf_backend_t backend = sensor_rngfnd_find_instance(orientation);
    if (backend == NULL) {
        return false;
    }
    return sensor_rf_get_temp(backend, temp);
}

bool sensor_rngfnd_prearm_healthy(char *failure_msg, const uint8_t failure_msg_len)
{
    for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) {
        if ((enum RangeFinderType)rangefinder.params[i].type == RANGEFINDER_NONE) {
            continue;
        }

        if (rangefinder.drivers[i] == NULL) {
            rt_snprintf(failure_msg, failure_msg_len, "Rangefinder %X: Not Detected", i + 1);
            return false;
        }

        switch (sensor_rf_status(rangefinder.drivers[i])) {
        case RangeFinderNoData:
            rt_snprintf(failure_msg, failure_msg_len, "Rangefinder %X: No Data", i + 1);
            return false;
        case RangeFinderNotConnected:
            rt_snprintf(failure_msg, failure_msg_len, "Rangefinder %X: Not Connected", i + 1);
            return false;
        case RangeFinderOutOfRangeLow:
        case RangeFinderOutOfRangeHigh:
        case RangeFinderGood:  
            break;
        }
    }

    return true;
}

// Write an RFND (rangefinder) packet
void sensor_rngfnd_log_rfnd()
{
    if (rangefinder._log_rfnd_bit == (uint32_t)(-1)) {
        return;
    }

    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
        sensor_rf_backend_const_t s = sensor_rngfnd_get_backend(i);
        if (s == NULL) {
            continue;
        }

        const struct log_RFND pkt = {
                APLOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
                .time_us      = time_micros64(),
                .instance     = i,
                .dist         = sensor_rf_distance_cm(s),
                .status       = (uint8_t)sensor_rf_status(s),
                .orient       = sensor_rf_orientation(s),
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
    }
}


/// 
static bool _add_backend(sensor_rf_backend_t backend, uint8_t instance, uint8_t serial_instance)
{
    if (!backend) {
        return false;
    }
    if (instance >= RANGEFINDER_MAX_INSTANCES) {
        console_panic("Too many RANGERS backends");
    }
    if (rangefinder.drivers[instance] != NULL) {
        // we've allocated the same instance twice
        INTERNAL_ERROR(internal_error_flow_of_control);
    }
    sensor_rf_init_serial(backend, serial_instance);
    rangefinder.drivers[instance] = backend;
    rangefinder.num_instances = MAX(rangefinder.num_instances, instance+1);

    return true;
}

/*
  detect if an instance of a rangefinder is connected. 
 */
static void detect_instance(uint8_t instance, uint8_t* serial_instance)
{
#if MB_RANGEFINDER_ENABLED
    sensor_rf_backend_t (*serial_create_fn)(sensor_rf_state_t _state, sensor_rf_params_t _params) = NULL;

    const enum RangeFinderType _type = (enum RangeFinderType)rangefinder.params[instance].type;
    switch (_type) {
    case RANGEFINDER_BenewakeTFmini:
#if MB_RANGEFINDER_BENEWAKE_TFMINI_ENABLED
        serial_create_fn = sensor_rf_backend_benewake_tfmini_create;
#endif
        break;

    case RANGEFINDER_BenewakeTF02:
#if MB_RANGEFINDER_BENEWAKE_TF02_ENABLED
        serial_create_fn = sensor_rf_backend_benewake_tf02_create;
#endif
        break;

    case RANGEFINDER_BenewakeTF03:
#if MB_RANGEFINDER_BENEWAKE_TF03_ENABLED
        serial_create_fn = sensor_rf_backend_benewake_tf03_create;
#endif
        break;
    case RANGEFINDER_NONE:
        break;
    default:
        console_printf("[rangefinder]: Unsupported rangefinder type(%d)", (int8_t)_type);
        gcs_send_text(MAV_SEVERITY_WARNING, "[rangefinder]: Unsupported rangefinder type(%d)", (int8_t)_type);
        break;
    }

    if (serial_create_fn != NULL) {
        if (SerialManager_have_serial(SerialProtocol_Rangefinder, (*serial_instance))) {
            sensor_rf_backend_t b = serial_create_fn(&rangefinder.state[instance], &rangefinder.params[instance]);
            if (b != NULL) {
                _add_backend(b, instance, (*serial_instance)++);
            }
        }
    }
#endif //MB_RANGEFINDER_ENABLED
}

//
static void sensor_rngfnd_assign_param()
{
    RF_PARAM_LINK_VARIABLE(rangefinder, 1);

#if RANGEFINDER_MAX_INSTANCES > 1
    RF_PARAM_LINK_VARIABLE(rangefinder, 2);
#endif

#if RANGEFINDER_MAX_INSTANCES > 2
    RF_PARAM_LINK_VARIABLE(rangefinder, 3);
#endif

#if RANGEFINDER_MAX_INSTANCES > 3
    RF_PARAM_LINK_VARIABLE(rangefinder, 4);
#endif

#if RANGEFINDER_MAX_INSTANCES > 4
    RF_PARAM_LINK_VARIABLE(rangefinder, 5);
#endif

#if RANGEFINDER_MAX_INSTANCES > 5
    RF_PARAM_LINK_VARIABLE(rangefinder, 6);
#endif

#if RANGEFINDER_MAX_INSTANCES > 6
    RF_PARAM_LINK_VARIABLE(rangefinder, 7);
#endif

#if RANGEFINDER_MAX_INSTANCES > 7
    RF_PARAM_LINK_VARIABLE(rangefinder, 8);
#endif

#if RANGEFINDER_MAX_INSTANCES > 8
    RF_PARAM_LINK_VARIABLE(rangefinder, 9);
#endif

#if RANGEFINDER_MAX_INSTANCES > 9
    RF_PARAM_LINK_VARIABLE2(rangefinder, A, 10);
#endif
}

/*------------------------------------test------------------------------------*/


